import threading
import cv2
import numpy as np
import gym_pih.hardware.TIS as TIS

class Camera():
    def __init__(self, width = 1280, height = 720, fps = 60):
        '''
        直接调用相机延迟太大了，这里用多线程读取图片
        '''
        self.tis = TIS.TIS()
        self.tis.open_device("33124075", width, height, f"{fps}/1", TIS.SinkFormats.BGRA, True)
        self.tis.set_property("ExposureAuto", "Off")
        self.tis.set_property("GainAuto", "Off")
        self.tis.set_property("ExposureTime", 16666)  # 1/60s
        self.tis.set_property("Gain", 5.2)
        # self.tis.set_property("ExposureTime", 8333)  # 1/120s
        # self.tis.set_property("Gain", 12.5)
        
        self.latest_image = None
        self.running = False
        self.lock = threading.Lock()
        self.thread = threading.Thread(target=self._update_image)

    def _update_image(self):
        while self.running:
            if self.tis.snap_image(1):
                image = self.tis.get_image()
                image = image[:, :, :3]
                with self.lock:
                    self.latest_image = image

    def Open(self):
        self.running = True
        self.tis.start_pipeline()
        self.thread.start()

    def Close(self):
        self.running = False
        self.thread.join()
        self.tis.stop_pipeline()

    def GetImage(self):
        with self.lock:
            return (True, self.latest_image.copy()) if self.latest_image is not None else (False, None)

    def SetProperty(self, property, value):
        '''
        设置相机参数
        Args:
            property: 相机属性名，如：ExposureAuto、GainAuto、ExposureTime、Gain
            value: 相机属性值

        Returns:None

        '''
        self.tis.set_property(property, value)
# Usage example
if __name__ == "__main__":
    camera = Camera()
    camera.Open()
    from gym_pih.hardware.LightSource import LightSource
    lightSource = LightSource()
    lightSource.Light_Open(6)
    camera.SetProperty("Gain",5.2)
    cv2.namedWindow("Camera Feed", cv2.WINDOW_NORMAL)

    try:
        while True:
            _,image = camera.GetImage()
            if image is not None:
                image = image[60:700, 260:900]
                image = cv2.resize(image, (128, 128))
                cv2.resizeWindow("Camera Feed", 640, 640)
                cv2.imshow("Camera Feed", image)
                if cv2.waitKey(1) & 0xFF == ord('q'):
                    break
    finally:
        camera.Close()
        lightSource.Light_Close()
        cv2.destroyAllWindows()
